/*
 * Filters.cpp
 *
 *  Created on: Feb 13, 2020
 *      Author: Yannick
 */

#include "Filters.h"
#include <math.h>

void BQ_Init(Biquad* bq,uint8_t type, float f, float Q, float peakGainDB)
{
    BQ_SetBiquad(bq,type, f, Q, peakGainDB);
}


/**
 * Sets the frequency
 * Calculate as f = f/samplerate
 * Must be lower than 0.5
 */
void BQ_SetFc(Biquad* bq,float f) 
{
	if(f > 0.5)
	{
		f = 0.5;
	}
	else if(f < 0)
	{
		f = 0.0;
	}
    bq->f = f;
    BQ_CalcBiquad(bq);
}

float BQ_GetFc(Biquad* bq)
{
	return bq->f;
}

/**
 * Changes bq->Q value and recalculaes filter
 */
void BQ_SetQ(Biquad* bq,float Q) {
    bq->Q = Q;
    BQ_CalcBiquad(bq);
}

float BQ_GetQ(Biquad* bq) {
	return bq->Q;
}

/**
 * Calculates one step of the filter and returns the output
 */
float BQ_Process(Biquad* bq,volatile float in) {
	float out;
	Biquad* _bq = bq;
	_bq->d0 = in - _bq->d1*_bq->a1 - _bq->d2*_bq->a2;
	if(isinf(_bq->d0) || isnan(_bq->d0))
	{
		_bq->d0 = in;
	}
	out = _bq->d0*_bq->b0 + _bq->d1*_bq->b1 + _bq->d2*_bq->b2;
	_bq->d2 = _bq->d1;
	_bq->d1 = _bq->d0;
	return out;
//	float out = in * bq->a0 + bq->d1;
//    bq->d1 = in * bq->a1 + bq->d2 - bq->b1 * out;
//    bq->d2 = in * bq->a2 - bq->b2 * out;
//    return out;
}

void BQ_SetBiquad(Biquad* bq,uint8_t type, float f, float Q, float peakGainDB) 
{
	if(f > 0.5)
	{
		f = 0.5;
	}
	else if(f < 0)
	{
		f = 0.0;
	}
	
    bq->type = type;
    bq->Q = Q;
    bq->f = f;
    bq->peakGain = peakGainDB;
    BQ_CalcBiquad(bq);
}

/*
 * Updates parameters and resets the biquad filter
 */
void BQ_CalcBiquad(Biquad* bq) {
	bq->d0 = 0.0;
	bq->d1 = 0.0;
	bq->d2 = 0.0;
    float norm;
    float V = pow(10, fabs(bq->peakGain) / 20.0);
    float ohm = tan(M_PI * bq->f);
    switch (bq->type) {
        case lowpass:
			bq->Q = 0.7071f;
            norm = 1.0f / (1.0f + ohm / bq->Q + ohm * ohm);
            bq->b0 = ohm * ohm * norm;
            bq->b1 = 2.0f * bq->b0;
            bq->b2 = bq->b0;
            bq->a1 = 2.0f * (ohm * ohm - 1.0f) * norm;
            bq->a2 = (1.0f - ohm / bq->Q + ohm * ohm) * norm;
            break;

        case highpass:
            norm = 1 / (1 + ohm / bq->Q + ohm * ohm);
            bq->b0 = 1 * norm;
            bq->b1 = -2 * bq->b0;
            bq->b2 = bq->b0;
            bq->a1 = 2 * (ohm * ohm - 1) * norm;
            bq->a2 = (1 - ohm / bq->Q + ohm * ohm) * norm;
            break;

        case bandpass:
            norm = 1 / (1 + ohm / bq->Q + ohm * ohm);
            bq->b0 = ohm / bq->Q * norm;
            bq->b1 = 0;
            bq->b2 = -bq->b0;
            bq->a1 = 2 * (ohm * ohm - 1) * norm;
            bq->a2 = (1 - ohm / bq->Q + ohm * ohm) * norm;
            break;

        case notch:
            norm = 1 / (1 + ohm / bq->Q + ohm * ohm);
            bq->b0 = (1 + ohm * ohm) * norm;
            bq->b1 = 2 * (ohm * ohm - 1) * norm;
            bq->b2 = bq->b0;
            bq->a1 = bq->b1;
            bq->a2 = (1 - ohm / bq->Q + ohm * ohm) * norm;
            break;

        case peak:
            if (bq->peakGain >= 0) {    // boost
                norm = 1 / (1 + 1/bq->Q * ohm + ohm * ohm);
                bq->b0 = (1 + V/bq->Q * ohm + ohm * ohm) * norm;
                bq->b1 = 2 * (ohm * ohm - 1) * norm;
                bq->b2 = (1 - V/bq->Q * ohm + ohm * ohm) * norm;
                bq->a1 = bq->b1;
                bq->a2 = (1 - 1/bq->Q * ohm + ohm * ohm) * norm;
            }
            else {    // cut
                norm = 1 / (1 + V/bq->Q * ohm + ohm * ohm);
                bq->b0 = (1 + 1/bq->Q * ohm + ohm * ohm) * norm;
                bq->b1 = 2 * (ohm * ohm - 1) * norm;
                bq->b2 = (1 - 1/bq->Q * ohm + ohm * ohm) * norm;
                bq->a1 = bq->b1;
                bq->a2 = (1 - V/bq->Q * ohm + ohm * ohm) * norm;
            }
            break;
        case lowshelf:
            if (bq->peakGain >= 0) {    // boost
                norm = 1 / (1 + sqrt(2) * ohm + ohm * ohm);
                bq->b0 = (1 + sqrt(2*V) * ohm + V * ohm * ohm) * norm;
                bq->b1 = 2 * (V * ohm * ohm - 1) * norm;
                bq->b2 = (1 - sqrt(2*V) * ohm + V * ohm * ohm) * norm;
                bq->a1 = 2 * (ohm * ohm - 1) * norm;
                bq->a2 = (1 - sqrt(2) * ohm + ohm * ohm) * norm;
            }
            else {    // cut
                norm = 1 / (1 + sqrt(2*V) * ohm + V * ohm * ohm);
                bq->b0 = (1 + sqrt(2) * ohm + ohm * ohm) * norm;
                bq->b1 = 2 * (ohm * ohm - 1) * norm;
                bq->b2 = (1 - sqrt(2) * ohm + ohm * ohm) * norm;
                bq->a1 = 2 * (V * ohm * ohm - 1) * norm;
                bq->a2 = (1 - sqrt(2*V) * ohm + V * ohm * ohm) * norm;
            }
            break;
        case highshelf:
            if (bq->peakGain >= 0) {    // boost
                norm = 1 / (1 + sqrt(2) * ohm + ohm * ohm);
                bq->b0 = (V + sqrt(2*V) * ohm + ohm * ohm) * norm;
                bq->b1 = 2 * (ohm * ohm - V) * norm;
                bq->b2 = (V - sqrt(2*V) * ohm + ohm * ohm) * norm;
                bq->a1 = 2 * (ohm * ohm - 1) * norm;
                bq->a2 = (1 - sqrt(2) * ohm + ohm * ohm) * norm;
            }
            else {    // cut
                norm = 1 / (V + sqrt(2*V) * ohm + ohm * ohm);
                bq->b0 = (1 + sqrt(2) * ohm + ohm * ohm) * norm;
                bq->b1 = 2 * (ohm * ohm - 1) * norm;
                bq->b2 = (1 - sqrt(2) * ohm + ohm * ohm) * norm;
                bq->a1 = 2 * (ohm * ohm - V) * norm;
                bq->a2 = (V - sqrt(2*V) * ohm + ohm * ohm) * norm;
            }
            break;
    }

    return;
}
